﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using ParticleTracker.Utilities;
using MathNet.Numerics;

namespace ParticleTracker
{
    class FrameCouple
    {
        public ParticleFrame frame1 { set; get; }
        public ParticleFrame frame2 { set; get; }
        public List<Velocity> velocities { private set; get; }
        public List<Rectangle> subFrames1 { private set; get; }
        public List<List<Rectangle>> subFrames2 { private set; get; }
        private double mu, std;
        public FrameCouple(List<Rectangle> subFrames1,
            List<List<Rectangle>> subFrames2)
        {
            this.subFrames1 = subFrames1;
            this.subFrames2 = subFrames2;
            velocities = new List<Velocity>(subFrames1.Count);
        }
        public void findVelocities()
        {
            velocities.Clear();
            //doStatistics();
            for (int i = 0; i < subFrames1.Count; i++)
            {
                Console.WriteLine("WORKING!!!  " + (double)i / subFrames1.Count * 100 + "%");
                velocities.Add(calcVel(i));
            }
            Console.WriteLine("woot mcPoot");
        }
        public Velocity calcVel(int i)
        {
            Complex[,] sub1, sub2, xCorr;
            ComplexFrame frame = new ComplexFrame(frame1);
            sub1 = frame.GetComplexRect(subFrames1[i]);
            // This loop figures out which window had the most
            // convincing xCorr and keeps its displacement.
            double max = 0;
            Point disp = new Point();
            for (int j = 0; j < subFrames2[i].Count; j++)
            {
                ComplexFrame frameN = new ComplexFrame(frame2);
                sub2 = frameN.GetComplexRect(subFrames2[i][j]);
                xCorr = XCorr2.xCorr2(sub1, sub2);
                // This loop locates the maximum value and the
                // indices of the maximum value in the cross
                // correlation of the two subframes.
                double localMax = 0;
                Point localDisp = new Point();
                //int nn, mm, xshift, yshift; //debug
                //Point sub2location, sub1location; // debug
                for (int m = 0; m < xCorr.GetLength(0); m++)
                {
                    for (int n = 0; n < xCorr.GetLength(1); n++)
                    {
                        if (xCorr[m, n].Real > localMax)
                        {
                            localMax = xCorr[m, n].Real;
                            // CAUTION!!! CAUTION!!!
                            // these displacements are still in the graphics 
                            // coordinate system.
                            // (up is -y, down is +y)
                            //debug
                            //nn = n;
                            //mm = m;
                            //xshift = (int)(xCorr.GetLength(1) / 2);
                            //yshift = (int)(xCorr.GetLength(0) / 2);
                            //sub2location = new Point(subFrames2[i][j].X, subFrames2[i][j].Y);
                            //sub1location = new Point(subFrames1[i].X, subFrames1[i].Y);
                            //debug


                            // DO NOT KNOW WHY YET, but have to flip these values.
                            localDisp.X = n - (sub1.GetLength(1) - 1) + subFrames2[i][j].X - subFrames1[i].X;
                            localDisp.Y = m - (sub1.GetLength(0) - 1) + subFrames2[i][j].Y - subFrames1[i].Y;
                            //localDisp.Y = n - (sub1.GetLength(1) - 1) + subFrames2[i][j].X - subFrames1[i].X;
                            //localDisp.X = m - (sub1.GetLength(0) - 1) + subFrames2[i][j].Y - subFrames1[i].Y;
                        }
                    }
                }
                if (localMax > max)
                {
                    max = localMax;
                    disp = localDisp;
                    //debug
                    //Console.WriteLine("\n\n\nxCorr");
                    //ArrayOps.printArray(xCorr);
                    //Console.WriteLine("Max = " + localMax);
                    //Console.WriteLine("Sub1");
                    //ArrayOps.printArray(sub1);
                    //Console.WriteLine("Sub2");
                    //ArrayOps.printArray(sub2);
                    //Console.WriteLine("disp");
                    //Console.WriteLine(localDisp);
                    //Console.WriteLine("");
                    //debug
                }
            }
            Complex[,] autoCorr = XCorr2.xCorr2(sub1, sub1);
            double auto = autoCorr[(int)(autoCorr.GetLength(0) * 0.5), (int)(autoCorr.GetLength(1) * 0.5)].Real;
            //double auto = 1;
            Velocity vel = new Velocity();
            vel.v = disp;
            //vel.nSigma = (max - mu) / std;
            vel.confidence = max / auto;
            return vel;
        }
        public void doStatistics()
        {
            Complex[,] sub, xCorr;
            double val;
            List<double> values = new List<double>();
            ComplexFrame frame = new ComplexFrame(frame1);
            for (int i = 0; i < subFrames1.Count; i++)
            {
                sub = frame.GetComplexRect(subFrames1[i]);
                xCorr = XCorr2.xCorr2(sub, sub);
                //Might want to only use values above a certain
                //threshold to calculate the mean and standard
                //deviation. Values obviously derived from zero
                //aren't really adding any information and skew
                //mu lower.
                //CAUTION!!!!!
                //CAUTION!!!!!
                //Gonna ignore values below 1 for now.
                //since this is an autocorrelation, the dimensions
                //of xCorr will be odd, and the max value will be at
                //zero displacement, which is the very middle of the
                //matrix, which happens to have the indices of 
                //(int)(Length/2);
                val = xCorr[(int)(xCorr.GetLength(0) * 0.5), (int)(xCorr.GetLength(1) * 0.5)].Real;
                if (val >= 1)
                {
                    values.Add(val);
                }
            }
            this.mu = calcMean(values);
            this.std = calcStd(values);
        }
        public double calcMean(List<double> vals)
        {
            return vals.Sum() / vals.Count;
        }
        public double calcStd(List<double> vals)
        {
            double sampV = 0;
            double dev;
            for (int i = 0; i < vals.Count; i++)
            {
                dev = vals[i] - mu;
                sampV += dev * dev;
            }
            sampV = 1 / ((double)vals.Count - 1) * sampV;
            return Math.Sqrt(sampV);
        }
    }
}